﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Media.Imaging;
using System.Windows.Media;
using Microsoft.Kinect;
using System.Threading;

namespace KinectGestureRecognition.ObjectDetection
{
    class DepthStreamObjectDetection : Interfaces.DepthStreamStrategy
    {
        public DepthStreamObjectDetection() : base("DepthStreamObjectDetection") { }

        #region members
        const int slotSizeFactor = 7;
        const int noOfSlots = 4096 >> slotSizeFactor;
        int[] SlotCounts = new int[noOfSlots];
        byte[] depthColorImage = null;
        short[] depthData = null;
        KinectSensor kinectSensorSender;
        #endregion

        public override void PerformDepthStreamOperation(object sender,DepthImageFrame depthImageFrame)
        {
            kinectSensorSender =  ((KinectSensor)sender);


            if (depthImageFrame == null)
            {
                return;
            }
            if (depthData == null)
                depthData = new short[depthImageFrame.PixelDataLength];

            if (depthColorImage == null)
                depthColorImage = new byte[depthImageFrame.PixelDataLength * 4];

        
            short[] pixelData = new short[depthImageFrame.PixelDataLength];
            depthImageFrame.CopyPixelDataTo(pixelData);
            int stride = depthImageFrame.Width * depthImageFrame.BytesPerPixel;

            Thread thread = new Thread(delegate() { setColorImage(depthData, depthImageFrame.Width, depthImageFrame.Height); });
            thread.Start();
            //setColorImage(depthData, depthImageFrame.Width, depthImageFrame.Height);
                
            //bitmapSource = BitmapSource.Create(depthImageFrame.Width, depthImageFrame.Height, 96, 96,
            //                                          PixelFormats.Gray16, null, pixelData, stride);

            bitmapSource = BitmapSource.Create(depthImageFrame.Width, depthImageFrame.Height, 96, 96,
                                                        PixelFormats.Gray16, null, pixelData, stride);            
                
        // Throw Event
            ThrowOnDepthStreamOperationEvent();
                
        }

        private void setColorImage(short[] depthData, int width, int height)
        {

            // Clear the depth counters in each slot
            for (int slotNo = 0; slotNo < SlotCounts.Length; slotNo++)
            {
                SlotCounts[slotNo] = 0;
            }

            // Make a pass through the image counting depth pixels

            for (int depthPos = 0; depthPos < depthData.Length; depthPos++)
            {
                int depthValue = depthData[depthPos] >> 3;
                // Check for the invalid values


                if (depthValue == kinectSensorSender.DepthStream.UnknownDepth ||
                    depthValue == kinectSensorSender.DepthStream.TooFarDepth ||
                    depthValue == kinectSensorSender.DepthStream.TooNearDepth)
                {
                    continue;
                }

                // Got a valid depth value
                // Find the slot and increase the counter

                int slotNumber = depthValue >> slotSizeFactor;
                SlotCounts[slotNumber]++;
            }

            // Now find the depth with the most pixels
            int maxCount = 0;
            int maxPos = 0;

            for (int slotNo = 0; slotNo < SlotCounts.Length; slotNo++)
            {
                if (SlotCounts[slotNo] > maxCount)
                {
                    maxCount = SlotCounts[slotNo];
                    maxPos = slotNo;
                }
            }

            // Now color those pixels red in the image

            int depthColorImagePos = 0;

            for (int depthPos = 0; depthPos < depthData.Length; depthPos++)
            {
                int depthValue = depthData[depthPos] >> 3;
                // Check for the invalid values

                if (depthValue == kinectSensorSender.DepthStream.UnknownDepth ||
                    depthValue == kinectSensorSender.DepthStream.TooFarDepth ||
                    depthValue == kinectSensorSender.DepthStream.TooNearDepth)
                {
                    // Color invalid pixels black
                    // Blue
                    depthColorImage[depthColorImagePos] = 0;
                    depthColorImagePos++;
                    // Green
                    depthColorImage[depthColorImagePos] = 0;
                    depthColorImagePos++;
                    // Red
                    depthColorImage[depthColorImagePos] = 0;
                    depthColorImagePos++;
                }
                else
                {
                    byte depthByte = (byte)(255 - (depthValue >> 4));

                    if (depthValue >> slotSizeFactor == maxPos)
                    {
                        // Blue
                        depthColorImage[depthColorImagePos] = 0;
                        depthColorImagePos++;
                        // Green
                        depthColorImage[depthColorImagePos] = 0;
                        depthColorImagePos++;
                        // Red
                        depthColorImage[depthColorImagePos] = depthByte;
                        depthColorImagePos++;
                    }
                    else
                    {
                        // Blue
                        depthColorImage[depthColorImagePos] = depthByte;
                        depthColorImagePos++;
                        // Green
                        depthColorImage[depthColorImagePos] = depthByte;
                        depthColorImagePos++;
                        // Red
                        depthColorImage[depthColorImagePos] = depthByte;
                        depthColorImagePos++;
                    }
                }
                // transparency
                depthColorImagePos++;
            }
        }
    }
}
